from Constants import *
from IsObstacleMet import *
from myro import *

direction = 0

def zigzagLeft():
    global direction
    direction=0
    horizontalDistance = 0
    readings=Readings()
    
    
    turnLeft90()
    
    while 1:
        startTime=currentTime()
        stop()
        readings.clear()
        if readings.takeReading()>THRESHOLD:
            turnLeft90() 
            continue
        forward(FORWARD_SPEED)
        elapsedTime=0
        
        while 1:    
            elapsedTime=currentTime()-elapsedTime
            if direction==1:
                horizontalDistance-=elapsedTime
            elif direction==3:
                horizontalDistance+=elapsedTime
            if horizontalDistance>0:
                turnLeft90()
                return             
            
            if readings.takeReading()>THRESHOLD:  
                turnLeft90()
                break
            elif (currentTime()-startTime)>FORWARD_TIME:
                turnRight90()
                break
                
def zigzagRight():
    global direction
    direction=0
    horizontalDistance = 0
    readings=Readings()
    
    
    turnRight90()
    
    while 1:
        startTime=currentTime()
        stop()
        readings.clear()
        if readings.takeReading()>THRESHOLD:
            turnRight90() 
            continue
        forward(FORWARD_SPEED)
        elapsedTime=0
        
        while 1:    
            elapsedTime=currentTime()-elapsedTime
            if direction==1:
                horizontalDistance-=elapsedTime
            elif direction==3:
                horizontalDistance+=elapsedTime
            if horizontalDistance<0:
                turnRight90()
                return             
            
            if readings.takeReading()>THRESHOLD:  
                turnRight90()
                break
            elif (currentTime()-startTime)>FORWARD_TIME:
                turnLeft90()
                break
    
    
    
    

def turnLeft90():
    global direction
    turnLeft(TURN_SPEED, TURN_TIME)
    direction+=1
    direction%=4

def turnRight90():
    global direction
    turnRight(TURN_SPEED, TURN_TIME)
    direction-=1
    direction%=4